



#include"dev_motor.h"

#include"gl_.h"  // 用来绘制速度曲线
#include<math.h>

///   下面是步进电机控制的代码。步进电机启动，加速度要连续可导        ////

/*
为简化控制逻辑，只有下面几个操作：
1、设定电机旋转到某个设定角度，速度会s型上升下降。  //这个在回退的时候用。
2、强制电机停止，速度会s形下降。                    //这个在暂停的时候用。
3、设置到某个速度。速度会s形变化到指定速度。        //用于变速。

3、电机在转动过程中，无法重新设定角度。只能先强制停止，再修改目标角度。
因此，前进的时候如果不清楚距离，可以设定一个能前进的最大值。
*/

//直接暴力设定每个点current_step对应的频率脉冲，来计算脉冲周期。周期会有些误差，但没关系了。

/*
速度变化函数：y = g(x) = ( sinf( (x-0.5)*3.14 ) + 1 )/2  // x=[0,1], g的值也在【0，1】，这里x是时间，g 是速度。
上式积分：y = h(x) = x/2 -sinf(3.14*x)/6.28      // x=0时y=0，x=2时y=1 .  x = 2时减速完成。这里x是时间，h是步长。
g(x) 在【0，1】的反函数： x(g) = arcsin( 2g - 1)/3.14 + 0.5    //asin(x) [数学表示为arcsin(x)] 函数定义域 [-1，1]
h(x) 在【0，2】的反函数： 卧槽！matlab也找不到这个函数的反函数。理论上反函数是存在的
h(x) 在定义域【0，1】泰勒展开：下面的式子在0，1的定义域拟合得挺好的。
(24649*x^3)/30000-(607573201*x^5)/1500000000+(14976071831449*x^7)/157500000000000
这玩意也没法求解析的反函数…… 次数太高，matlab给出的是root函数的解。这里就直接查表计算反函数了。

那先得到h(g)的表达式吧,将x(g)带入h(x)：得到步长h与速度g的关系函数。
h(g)  = (arcsin( 2*g - 1)/3.14 + 0.5)/2 - sinf(3.14* (arcsin( 2*g - 1)/3.14 + 0.5) )/6.28   【 g={0,1},h={0,0.5} 单调增。 】
// 为了画图：
(arcsin( 2*x - 1)/3.14 + 0.5)/2 - sinf(3.14* (arcsin( 2*x - 1)/3.14 + 0.5) )/6.28

#define Max_Frequency     //脉冲最大频率（次/秒）
#define Min_Frequency     //脉冲最小频率
步进距离 endstep (步）一个脉冲一步
最大加速时间 T0 = 3   //加速时间（秒为单位，这里是从最低速加到最高速的时间）
对应的加速的步数 S0   // T0 和S0应该会有一个函数关系。

如果S0减小，就减小最大速度。加速时间不变，除非步数比最低速运行的加速时间还少。这里就分类讨论了。

3秒从最低速加到最高速，可以得到时间t与频率的关系：
频率 f =g(t/T0) * (Max_Frequency - Min_Frequency) + Min_Frequency .t是秒的单位。
理想情况下：加速时间（包括停止前的减速）的步数 S0 =  h(2*T0/T0) * (Max_Frequency - Min_Frequency) * T0 + Min_Frequency * T0  = Max_Frequency * T0.  这个表达式真是太好了。

因此：总步数超过 Max_Frequency * T0 的，中间用最大速度行驶。
开始加速，当时间不到T0时（步数少于Max_Frequency * T0/2 时） ，步数的频率与时间相关：需要记录一下加速开始的时间。 这里最好能得到 累计步数与频率的关系。
累计步数，即对时间的积分，前面有积分公式h(x/T0),x表示的是时间，h(x/T0)是累计步数，但g(x/T0)是当前的频率！
即将函数化成 g(h) 即可,即得到当前累计步数下，下一步的频率。 h(g)是可以得到的，g=【0，1】表示速度范围，h=【0，0.5】表示加速步数的百分比。
h = 当前累计步数 / Max_Frequency * T0 / 2。

方案1：
我们要根据h(g) 计算g（h）。h(g)是已知的，且随g单调递增，h也已知，要计算g。直接二分法迭代求解。
或者事先计算好表，g以1/1024的精度计算出 h[g] 序列，计算反函数g(h)时，直接二分法查表h[g],然后线性差值。
g是速度，h是步数。
方案2：
先用二分法计算每个h对应的g的精确值，然后直接查表。
*/

/*
上述用步数对应速度的方式有个好处：波形的占空比能稳定到50%，而且不需要经常计算频率。
但有个问题：在变化方向加速的时候，速度无法降到零，速度到零后，是永远没有下一步的。

因此，用时间和速度联系起来比较合理。
用u32 记录步进电机运行时间，可以记录4,200,000,000个数，如果是微妙的单位，就有4,200秒，70分钟。一般的使用场景够了。
如果时间间隔是5us，就可以记录近6个小时。

这有另一个问题：每个5us对应的速度都会变化，而motor_driver_one_step 不一定能转动成功。
导致无法确定什么时候速度为零，无法确定加速时间。
这里采用另一个策略：
每个时间间隔，要计时间，并计算对应的速度。
如果速度不是零，就步进一步，如果成功步进，就记录当前步进成功的时间。
每个时间周期，刷新速度，及其对应的脉冲周期，如果当前的时间距离上次步进的时间大于脉冲周期，步进一步。

这里的脉冲电平是固定的，就是时间间隔不一样。按照手册的说法这是没问题的。不过计算量比较大，每个时间间隔得更新频率。


这样就只能加减速到指定速度。不能指定位置。
指定位置的功能要用前面的方案。

不过现在需要用到的是指定速度的方案。需要到某个速度匀速转。
回退的时候才是指定角度。而且两个方案不可混用，不然速度曲线不平滑。
等等，其实是可以混用的……加速的时候，按照时间加速。减速的时候，按照步数减速。 不好搞，速度模式还是按照时间来确定速度，反正不用限制角度。

加速过程中不能变化速度，这里加速的时间设到0.5秒，实时性挺高了。
设置一个目标速度变量，并检测这个变量有无更新。
和一个当前速度变量。 在加速完成，匀速或者静止阶段检查这个目标速度有无变化，如果有变化，则加速到目标速度。
这个方案比较适合时间决定速度的模式。这时只要将速度清零，就可调整目标速度了~


*/


/// <summary>
/// 已经设定了加速曲线，见上面一大串注释。可以得到 每个速度g与当前步长h 的关系。
/// g=【0，1】表示速度范围，h=【0，0.5】表示加速步数的百分比。
/// </summary>
/// <param name="g">输入速度g</param>
/// <returns>g对应的步长h</returns>
float func_hg(float g) {
	if (g < 0) g = 0;
	if (g >= 1) g = 1;
	float h =
		(asin(2 * g - 1) / 3.14159 + 0.5) / 2 - sinf(3.14159 * (asin(2 * g - 1) / 3.14159 + 0.5)) / 6.28318;
	return h;
}

/// <summary>
/// 二分法查找反函数。已知函数 y = f(x), 先已知因变量y，求自变量x。
/// f(x) 必须是单调递增的。否则会出错。
/// </summary>
/// <param name="func_x">已知的函数</param>
/// <param name="y">已知的因变量</param>
/// <param name="x_min">自变量的最大值</param>
/// <param name="x_max">自变量的最小值</param>
/// <param name="precision">计算精度</param>
/// <returns>自变量x</returns>
float BinarySearch_inverse_function(float (*func_x)(float), float y, float x_min, float x_max, float precision)
{
	if (precision < 0.00001f)precision = 0.00001f;

	if (func_x(x_min) >= y)
	{
		return x_min;
	}
	if (func_x(x_max) <= y)
	{
		return x_max;
	}

	float mid = (x_min + x_max) / 2;
	float fm = func_x(mid);
	while (ABS(fm - y) > precision) {
		if (y > fm) {
			x_min = mid;
			mid = (x_min + x_max) / 2;
			fm = func_x(mid);
		}
		else if (y < fm) {
			x_max = mid;
			mid = (x_min + x_max) / 2;
			fm = func_x(mid);
		}

		if (x_max - x_min < precision)break;
	}
	return mid;
}
//

#define gh_length 128
float gh[gh_length + 1] = { -3 };  // gh 的数组缓存。
/// <summary>
/// 根据函数 func_hg  h(g) 计算 反函数 g(h) ,主要用来计算g(h)的表，加快后续的运行速度。precision：计算精度。 
/// 还有：g=【0，1】表示速度范围，h=【0，0.5】表示加速步数范围，函数是单调递增的。这里只能用二分法查找。
/// </summary>
void init_gh()
{
	for (int i = 0; i < gh_length; i++) {
		//	gh[i] = func_gh_by_func_hg(0.5f * i / 1022, Epsilon);
		gh[i] = BinarySearch_inverse_function(func_hg, 0.5f * i / (gh_length - 1), 0, 1, 0.00001f);
	}
	gh[gh_length] = 1;
}
//

/// <summary>
/// 已经设定了加速曲线，见上面一大串注释。可以得到 每个速度g与当前步长h 的关系。
/// g=【0，1】表示速度范围，h=【0，0.5】表示加速步数的百分比。
/// </summary>
/// <param name="h">输入加速步数百分比</param>
/// <returns>速度g</returns>
float func_gh(float h)
{
	if (h >= 0.5)return 1;
	if (h < 0)return 0;
	// 在gh数组中找到最接近h的g。
	float dhi = h * 2 * (gh_length);   // 数组长度是1024
	int hi = (int)dhi;
	dhi = dhi - hi;
	float g = gh[hi] * (1 - dhi) + gh[hi + 1] * dhi;
	return g;
}
//

/// <summary>
/// 已经设定了加速曲线，见上面一大串注释。可以得到 每个速度g与当前步长h 的关系。
/// g=【0，1】表示速度范围，h=【0，0.5】表示加速步数的百分比。
/// 这里是快速版，直接计算索引，不进行线性插值。
/// </summary>
/// <param name="h">输入加速步数百分比</param>
/// <returns>速度g</returns>
float func_gh_fast(float h)
{
	if (h >= 0.5f)return 1;
	if (h < 0)return 0;
	// 在gh数组中找到最接近h的g。
	int i = (int)(h * gh_length * 2);
	//	if (i >= gh_length)i = gh_length - 1;
	return gh[i];
}
//

#define sin_table_length  128
#define sin_table_length_mul_2  256
#define sin_table_length_mul_4  512

float sin_table[sin_table_length + 1] = { -3 };    // sin 函数只记录 0 - pi/2 的值，其他值对称计算。

void init_sin_table()
{
	for (int i = 0; i < sin_table_length; i++) {
		sin_table[i] = sinf((float)i / sin_table_length * 6.2831853f / 4);
	}
	sin_table[sin_table_length] = 1;
}

float fast_sin(float theta) {
	// theta 以 0 - 2pi 为周期。 先缩放到 0-1 的周期，方便计算。
	theta /= 6.2831853f;
	bool flag = theta > 0 ? true : false;
	float x;
	int ix, h;
	// 在 sin_table 数组中找到最接近的，这里就不线性差值了。
	if (flag) {
		x = (theta - (int)theta);
		ix = x * sin_table_length_mul_4;
		if (x <= 0.25f) {
			return sin_table[ix];
		}
		else if (x <= 0.5f)
		{
			h = sin_table_length_mul_2 - ix;
			return sin_table[h];
		}
		else if (x <= 0.75f)
		{
			h = ix - sin_table_length_mul_2;
			return -sin_table[h];
		}
		else
		{
			h = sin_table_length_mul_4 - ix;
			return -sin_table[h];
		}
	}
	else
	{
		x = ((int)theta - theta);
		ix = x * sin_table_length_mul_4;
		if (x <= 0.25f) {
			return -sin_table[ix];
		}
		else if (x <= 0.5f)
		{
			h = sin_table_length_mul_2 - ix;
			return -sin_table[h];
		}
		else if (x <= 0.75f)
		{
			h = ix - sin_table_length_mul_2;
			return sin_table[h];
		}
		else
		{
			h = sin_table_length_mul_4 - ix;
			return sin_table[h];
		}
	}

}



void test_fast_sin(void)
{
	init_sin_table();

	float sum_err = 0;
	float max_err = 0;
	for (int i = 0; i < 5000; i++) {
		float x = i / 600.0f - 6;
		float fs = fast_sin(x);
		float s = sinf(x);
		//	printfs(" \r\n  %f  ", ABS(fs - s));
		sum_err += ABS(fs - s);
		max_err = max_err > ABS(fs - s) ? max_err : ABS(fs - s);
	}
	printfs(" \r\n sum_err = %f , max_err = %f ", sum_err, max_err);


}

// 测试加速度曲线的函数值。
void test_moto_curve()
{
#define moto_curve_len 30 
	float hx[moto_curve_len];
	float gx[moto_curve_len];

	float h[moto_curve_len];
	float g[moto_curve_len];
	init_gh();

	for (int i = 0; i < moto_curve_len; i++) {
		hx[i] = 1.0f / (moto_curve_len - 1) * i;
		h[i] = func_hg(hx[i]);

		gx[i] = 0.5f / (moto_curve_len - 1) * i;
		g[i] = func_gh(gx[i]);

		g[i] = func_gh_fast(gx[i]);

		//	printfs(" \r\n %f,  %f  ,%f  ,%f  , ", hx[i], h[i], gx[i], gh[i]);
	}

	TwoFunctionPicture tp;
	TwoFunctionPicture_init(&tp, 10, 20, 700, 450);
	char title[] = "Curve Chart";          //标题
	char xtitle[] = "xtitle time";          //x轴标题
	char y1title[] = "y1title cm";          //y轴标题
	char y2title[] = "y2title km";          //y轴标题
	TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);

	// 绘图显示。
	TwoFunctionPicture_draw_func(&tp,
		hx, h, moto_curve_len,
		gx, g, moto_curve_len,
		1, 1, 0, NULL, NULL);

	GD_wait_key_input();
}


// 角度模式，加减速的步数是固定的，只保存加减速节点的值。
// 速度模式，只有加速，这里的步数也是可以固定的。再留一个变量，保存是否加速中即可。


/// <summary>
/// 初始化步进电机
/// </summary>
/// <param name="steper_motor">步进电机结构体</param>
void steper_motor_init(steper_motor* sm)
{


//	if (gh[0] == -3) 
	{
		init_gh();
	}
//	if (sin_table[0] == -3) 
	{
		init_sin_table();
	}

	sm->max_step = nt32_MAX_number;
	sm->min_step = nt32_MIN_number;

	sm->current_frequency = 0;
	sm->current_step = 0;
	sm->target_frequency = 0;
	sm->target_step = 0;
	sm->start_frequency = 0;
	sm->start_step = 0;
	sm->new_frequency = 0;
	sm->new_step = 0;

	sm->mode = 2;
	sm->timer_count = 0;
	sm->timer_count_next_step = 1;

	sm->is_step = true;
	sm->new_dir = true;
	sm->io_dir = true;
	sm->io_enable = false;
	sm->io_pulse = false;
}


/// <summary>
/// 刷新步数，同时刷新方向。
/// </summary>
/// <param name="sm">步进电机的结构体</param>
void steper_motor_renew_step(steper_motor* sm)
{
	if (sm->new_step > sm->max_step)sm->new_step = sm->max_step;
	if (sm->new_step < sm->min_step)sm->new_step = sm->min_step;

	if (sm->mode == 0 && sm->current_step == sm->target_step) {  // 已经到达目标步数了。这时转速会降到零。
		//下面设置new_step
		if (sm->target_step != sm->new_step) {
			if (sm->target_step < sm->new_step)sm->new_dir = 1;
			else sm->new_dir = 0;
			sm->mode = 0;
			sm->timer_count = 0;
			sm->timer_count_next_step = 1;
			sm->start_step = sm->target_step;
			sm->target_step = sm->new_step;
		}
	}
	else if (sm->mode == 2 && sm->current_step != sm->new_step) {  // 停止状态。
		if (sm->current_step < sm->new_step)sm->new_dir = 1;
		else sm->new_dir = 0;
		sm->mode = 0;
		sm->timer_count = 0;
		sm->timer_count_next_step = 1;
		sm->start_step = sm->current_step;
		sm->target_step = sm->new_step;
	}
}

/// <summary>
/// 刷新频率，同时刷新方向。
/// </summary>
/// <param name="sm">步进电机的结构体</param>
void steper_motor_renew_frequency(steper_motor* sm)
{
	if (sm->mode == 1 && sm->current_frequency == sm->target_frequency) {  // 已经到达目标频率了。
		//下面设置new_frequency
		if (sm->target_frequency != sm->new_frequency) {
			if (ABS(sm->new_frequency) < motor_MIN_frequency)sm->new_frequency = 0;  // 如果转速过低，设为0.

			// 要先保持频率方向一致
			int newf = sm->new_frequency;
			if (sm->target_frequency > 0) {  // 转换速度方向，速度先将为0
				if (sm->new_frequency < 0) {
					newf = 0;
				}
			}
			if (sm->target_frequency < 0) {
				if (sm->new_frequency > 0) {
					newf = 0;
				}
			}

			sm->mode = 1;
			sm->timer_count = 0;
			sm->timer_count_next_step = 1;
			sm->start_frequency = sm->target_frequency;
			sm->target_frequency = newf;
			sm->breke_step = sm->target_frequency * motor_ACC_Time_s * 2;
		}
	}
	else if (sm->mode == 2 && ABS(sm->new_frequency) > motor_MIN_frequency) {  // 停止状态。
		sm->mode = 1;
		sm->timer_count = 0;
		sm->timer_count_next_step = 1;
		sm->start_frequency = 0;
		sm->target_frequency = sm->new_frequency;
		sm->breke_step = sm->target_frequency * motor_ACC_Time_s * 2;

		if (sm->new_frequency > 0)sm->new_dir = 1;
		else sm->new_dir = 0;
	}
}

/// <summary>
/// mode 0 :步数为准。
/// 这里要讨论整个步数没那么多的情况，这时速度无法加到最大速度。即总步数小于 Max_Frequency * T0。
/// 这时要降低当前实际的最大步数。已经很显然了。
/// 当实际最大步数比最低速度还小，就降低时间了。
/// 如果有设置新的角度，必须等当前角度转完，电机静止的状态。
/// </summary>
/// <param name="timer_us">脉冲生成器的时间周期，单位是us</param>
/// <param name="acc_time_s">变化速度的时间，单位是秒。</param>
/// <param name="max_f">最大的频率，看步进电机的参数</param>
/// <param name="min_f">最小的频率，一般设20，避免低频的时候一直不转</param>
/// <param name="start_step">开始的步数</param>
/// <param name="end_step">目标步数</param>
/// <param name="current_step">当前步数</param>
/// <returns>当前时间计数下的脉冲周期的计数。（*timer_us就是周期了）如果周期计数大于1000000，就认为静止</returns>
int steper_motor_get_pulse_period_mode_0(int timer_us, float acc_time_s, int max_f, int min_f,
	int start_step, int end_step, int current_step)
{
	int ALL_ACC_STEPS = acc_time_s * max_f;
	int HALF_ACC_STEPS = ALL_ACC_STEPS / 2;


	float a_e_s = ABS(end_step - start_step);
	float a_c_s = ABS(current_step - start_step);
	float a_c_e = ABS(current_step - end_step);

	if (a_e_s <= a_c_s) {   // 已经转到位了，停止转动
		return 2000000000;  // 如果是us， = 2000秒 约 30min。相当于静止了。
	}


	float frequency;
	if (a_e_s >= ALL_ACC_STEPS) { //常规操作

		if (a_c_s < HALF_ACC_STEPS) { //加速
			float h = a_c_s / HALF_ACC_STEPS;    // 
			frequency = func_gh_fast(h) * (max_f - min_f) + min_f;
		}
		else if (a_c_e < HALF_ACC_STEPS) {   // 减速
			float h = (a_c_e - 1) / HALF_ACC_STEPS;
			frequency = func_gh_fast(h) * (max_f - min_f) + min_f;
		}
		else {  //匀速
			frequency = max_f;
		}
	}

	else if (a_e_s < ALL_ACC_STEPS && a_e_s > min_f * acc_time_s) {  // 需要自适应最大速度。
		int maxf = a_e_s / acc_time_s;  // 最大速度是 ABS(end_s - start_s) / ACC_Time_s。 即没有匀速的阶段。
		if (a_c_s < a_c_e) { //加速
			float h = a_c_s / a_e_s;
			frequency = func_gh_fast(h) * (maxf - min_f) + min_f;
		}
		else {  // 减速
			float h = a_c_e / a_e_s;
			frequency = func_gh_fast(h) * (maxf - min_f) + min_f;
		}
	}

	else {  // 匀速运动。
		frequency = min_f;
	}

	return (1000000.0f / frequency) / timer_us;
}
//

/// <summary>
/// mode 1:频率为准
/// 在速度模式下，获取脉冲周期的计数。
///  g(x) = ( sinf(x-0.5)*3.14 + 1 )/2  // x=[0,1], g的值也在【0，1】，这里x是时间，g 是速度。
/// 如果要变换方向，必须先将速度降到零。    
/// </summary>
/// <param name="timer_us">脉冲生成器的时间周期，单位是us</param>
/// <param name="acc_time_s">变化速度的时间，单位是秒。</param>
/// <param name="max_f">最大的频率，看步进电机的参数</param>
/// <param name="min_f">最小的频率，一般设20，避免低频的时候一直不转</param>
/// <param name="start_speed">开始的速度(这里速度=频率)</param>
/// <param name="end_speed">目标速度</param>
/// <param name="start_timer_count">开始变速的时间计数，一般是0</param>
/// <param name="current_timer_count">当前的时间计数（如果初始时间计数是0，当前时间就是 current_timer_count*timer_us ）</param>
/// <param name="current_speed">当前的速度。这个参数由该函数进行刷新</param>
/// <returns>当前时间计数下的脉冲周期的计数。（*timer_us就是周期了）如果周期计数大于1000000，就认为静止</returns>
int steper_motor_get_pulse_period_mode_1(int timer_us, float acc_time_s, int max_f, int min_f,
	int* start_speed, int* end_speed, int start_timer_count, int current_timer_count, int* current_speed)
{
	//下面要确保运动方向是一致的。
	if (*start_speed > 0 && *end_speed < 0)*end_speed = 0;
	if (*start_speed < 0 && *end_speed > 0)*end_speed = 0;
	// 下面要确保速度在范围内。
	if (ABS(*start_speed) < min_f) {
		// 初始速度不能太小，否则启动慢。
		if (*end_speed > 0) *start_speed = min_f;
		else if (*end_speed < 0) *start_speed = -min_f;
		else {     // 如果初始速度很小，目标速度也是0，直接停止。
			return 2000000000;  // 如果是us， = 2000秒 约 30min。相当于静止了。
		}
	}
	if (ABS(*end_speed) > max_f) {
		if (*end_speed > 0) *end_speed = max_f;
		if (*end_speed < 0) *end_speed = -max_f;
	}
	// 如果目标速度太小，就设为零。
	if (ABS(*end_speed) < min_f) {
		*end_speed = 0;
	}

	float acc_timer_count = acc_time_s * 1000000 / timer_us;
	if ((current_timer_count - start_timer_count) >= acc_timer_count) {  //过了加速时间，直接返回目标的速度对应的周期
		if (*end_speed == 0) {
			*current_speed = 0;
			return 2000000000;  // 如果是us， = 2000秒 约 30min。相当于静止了。
		}
		else
		{
			*current_speed = *end_speed;
			if (*end_speed < 0) return (-1000000.0f / *end_speed) / timer_us;
			else return (1000000.0f / *end_speed) / timer_us;
		}
	}

	// 168MHz主频的STM32F405RGT6中，运行一次sin函数大约需要30~50us，运行一次arm_sin_f32函数，只需要3us。 sin函数还是查表吧……

	float frequency;
	float x = (float)(current_timer_count - start_timer_count) / acc_timer_count;
	float g = (fast_sin((x - 0.5f) * 3.14f) + 1) / 2;
	if (*end_speed == 0)
		frequency = g * (min_f - ABS(*start_speed)) + ABS(*start_speed);
	else
		frequency = g * (*end_speed - *start_speed) + *start_speed;

	if (ABS(frequency) < min_f) {
		if (*end_speed == 0) {
			*current_speed = 0;
			return 2000000000;  // 如果是us， = 2000秒 = 30min。相当于静止了。
		}
		else if (frequency > 0)frequency = min_f;
		else frequency = -min_f;
	}
	if (ABS(frequency) > max_f) {
		if (frequency > 0)frequency = max_f;
		else frequency = -max_f;
	}

	*current_speed = frequency;
	if (frequency < 0) return ((-1000000.0f / frequency) / timer_us);
	else return ((1000000.0f / frequency) / timer_us);
}
//

// 测试通过，生成的周期都是没问题的。
void test_get_pulse_period()
{
	init_gh();
	init_sin_table();
	// 
#define s_len 200
	float x[s_len];
	float y[s_len];
	float z[s_len];
	int t = 0;
	int ct = 0;
	for (int i = 0; i < s_len; i++) {
		y[i] = steper_motor_get_pulse_period_mode_0(50, 0.1, 600, 20,
			s_len, 0, s_len - i);
		if (y[i] > 1000000)y[i] = 0;
		int s = 0, e = 100;
		z[i] = steper_motor_get_pulse_period_mode_1(50, 0.2, 600, 20,
			&s, &e, 0, t, &ct);
		if (z[i] > 1000000)z[i] = 0;
		t += z[i];

		x[i] = i;

		printfs("  \r\n  time_us = %9d, f = %9f, f = %9f", i, y[i], z[i]);
	}

	TwoFunctionPicture tp;
	TwoFunctionPicture_init(&tp, 0, 0, 700, 450);
	char title[] = "Curve Chart";          //标题
	char xtitle[] = "xtitle time";          //x轴标题
	char y1title[] = "y1title cm";          //y轴标题
	char y2title[] = "y2title km";          //y轴标题
	TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);

	// 绘图显示。
	TwoFunctionPicture_draw_func(&tp,
		x, y, s_len,
		x, z, s_len,
		1, 0, 0, NULL, NULL);


	GD_wait_key_input();

}
//

/// <summary>
/// 驱动步进电机产生一个步进脉冲。
/// new_dir==1 顺时针转动一步。new_dir==0 逆时针转动一步。
/// is_step==1：成功转1步。is_step==0：还未转动完成。
/// 这个函数放定时器中，检测到 sm->is_step = false 就调用该函数。
/// 如果需要步进一步，在外部将sm->is_step 设为false即可。
/// 如果方向不对，最多调用3次变能使 sm->is_step 变成true。
/// </summary>
/// <param name="sm">步进电机结构体</param>
void steper_motor_make_one_step(steper_motor* sm)  //dir:方向 
{
	// new_dir==1 顺时针转动一步。new_dir==0 逆时针转动一步。is_step==1：成功转1步。is_step==0：还未转动完成。

	if (sm->io_enable == false) {
		sm->io_enable = true;    // 打开引脚使能。
	}
	else
	{

		// 默认电平：低电平。这里脉冲切换到低电平才有效。
		if (sm->new_dir == 1) {
			if (sm->io_dir == 1) { //方向对
				if (sm->io_pulse == 0) { //电平对
					sm->io_pulse = 1;    // 引脚置1，产生一个脉冲。
					sm->is_step = false;
				}
				else {
					if (sm->timer_count >= sm->timer_count_io_pulse_down) {
						sm->io_pulse = 0;   //电平恢复,引脚置零.
						sm->current_step++;
						sm->is_step = true;
					}
					else
					{
						// 
					}
				}
			}
			else {			//改变方向
				sm->io_dir = 1;
				sm->is_step = false;
			}
		}
		else {
			if (sm->io_dir == 0) {       //方向对
				if (sm->io_pulse == 0) { //电平对
					sm->io_pulse = 1;    // 引脚置1，产生一个脉冲。
					sm->is_step = false;
				}
				else {
					if (sm->timer_count >= sm->timer_count_io_pulse_down) {
						sm->io_pulse = 0;   //电平恢复,引脚置零.
						sm->current_step--;
						sm->is_step = true;
					}
					else
					{
						// 
					}
				}
			}
			else {			//改变方向
				sm->io_dir = 0;
				sm->is_step = false;
			}
		}
	}

}
//


/// <summary>
/// 在定时器中调用
/// 定时间隔要求 motor_timer_us < (1,000,000 / motor_MAX_frequency) / 5
/// </summary>
/// <param name="sm">步进电机的结构体</param>
void steper_motor_clock(steper_motor* sm)
{
	int next_period = 0;
	sm->timer_count++;
	if (sm->is_step == false) {
		steper_motor_make_one_step(sm);
	}
	// 先步进一步。
	if (sm->timer_count_next_step == sm->timer_count) {
		if (sm->mode == 0) {
			next_period = steper_motor_get_pulse_period_mode_0(
				motor_timer_us, motor_ACC_Time_s, motor_MAX_frequency, motor_MIN_frequency,
				sm->start_step, sm->target_step, sm->current_step);
			sm->timer_count_next_step += next_period;
			// 判断是否静止
			if (next_period > 1000000) {
				if (sm->target_step == sm->new_step) {
					sm->io_enable = false;
					sm->mode = 2;
				}
				else
				{
					steper_motor_renew_step(sm);
				}
			}
			else {
				// 步进一步。
				sm->is_step = false;
				sm->timer_count_io_pulse_down = sm->timer_count_next_step - (next_period >> 1); // 50% 占空比
			}
		}
		else if (sm->mode == 1) {

			next_period = steper_motor_get_pulse_period_mode_1(
				motor_timer_us, motor_ACC_Time_s, motor_MAX_frequency, motor_MIN_frequency,
				&sm->start_frequency, &sm->target_frequency, 0, sm->timer_count, &sm->current_frequency);
			sm->timer_count_next_step += next_period;

			//	printfs("  \r\n  %d   %d    %d   ", next_period , sm->timer_count_next_step, sm->timer_count);

			if (next_period > 1000000) { // 判断是否静止，静止即停止。
				sm->io_enable = false;
				sm->mode = 2;
				sm->target_step = sm->current_step;  //  更新目标步数，防止停止后自动回退
				sm->new_step = sm->current_step;  //  

				steper_motor_renew_frequency(sm);
			}
			else {
				// 步进一步。
				sm->is_step = false;
				sm->timer_count_io_pulse_down = sm->timer_count_next_step - (next_period >> 1); // 50% 占空比
			}
			if (sm->current_frequency == sm->target_frequency) {   // 到达设定频率，看看有无新的速度
				steper_motor_renew_frequency(sm);
			}

			// 频率模式要判断刹车距离。 如果小于刹车距离，设速度为零。

			if (sm->io_dir == true && sm->current_step + sm->breke_step >= sm->max_step) {
				sm->new_frequency = 0;
			}
			if (sm->io_dir == false && sm->current_step - sm->breke_step <= sm->min_step) {
				sm->new_frequency = 0;
			}

		}
	}
	if (sm->mode == 2) {    // 停止状态，判断是否有刷新变量
		if (sm->target_step == sm->current_step && sm->new_step != sm->target_step) {  //
			steper_motor_renew_step(sm);
		}
		if (sm->target_frequency == sm->current_frequency && sm->new_frequency != sm->target_frequency) {
			steper_motor_renew_frequency(sm);
		}
	}

}
//




// 下面测试步进电机驱动代码。
void test_steper_motor()
{
	// 测试几种加减速模式，记录波形。

#define motor_len 400  // 80    //  2000

	float z[motor_len];
	float y[motor_len];
	float x[motor_len];
	for (int i = 0; i < motor_len; i++) {
		x[i] = i;
		y[i] = 0;
		z[i] = 0;
	}

	steper_motor sm;
	steper_motor_init(&sm);
	sm.current_step = 0;

	TwoFunctionPicture tp;
	TwoFunctionPicture_init(&tp, 0, 0, 700, 450);
	char title[] = "Curve Chart";          //标题
	char xtitle[] = "xtitle time";          //x轴标题
	char y1title[] = "y1title cm";          //y轴标题
	char y2title[] = "y2title km";          //y轴标题
	TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);

	//1、设定一个目标位置。 正反向
	if (1) {
		sm.new_step = -120;
		//	steper_motor_renew_step(&sm);
		for (int i = 0; i < motor_len; i++) {
			y[i] = 0;
			while (1)
			{
				y[i]++;
				bool io_pulse = sm.io_pulse;
				steper_motor_clock(&sm);
				if (sm.io_enable == false)
				{
					break;
				}
				if (sm.io_pulse != io_pulse && sm.io_pulse) {
					break;
				}
			}
			z[i] = sm.io_dir;
		}
		// 绘图显示。
		TwoFunctionPicture_draw_func(&tp,
			x, y, motor_len,
				x, z, motor_len,
		//	NULL, NULL, 0,
			1, 0, 0, NULL, NULL);
		printfs(" \r\n 1 current step = %d current speed = %d ", sm.current_step, sm.current_frequency);

		GD_wait_key_input();
	}


	//2、设置两个目标位置。正反向。
	if (1) {
		//	 steper_motor_init(&sm);
		int j = 0;
		sm.new_step = -40;
		// steper_motor_renew_step(&sm);
		for (int i = 0; i < motor_len; i++) {

			if (sm.current_step == sm.new_step) {
				j++;
				if (j == 10)
					sm.new_step = 40;
			}

			y[i] = 0;
			while (1)
			{
				y[i]++;
				bool io_pulse = sm.io_pulse;
				steper_motor_clock(&sm);
				if (sm.io_enable == false)
				{
					break;
				}
				if (sm.io_pulse != io_pulse && sm.io_pulse) {
					break;
				}
			}
			z[i] = sm.io_dir;
		}
		printfs(" \r\n current step = %d current speed = %d ", sm.current_step, sm.current_frequency);
		// 绘图显示。
		TwoFunctionPicture_draw_func(&tp,
			x, y, motor_len,
			x, z, motor_len,
			//	NULL, NULL, 0,
			1, 0, 0, NULL, NULL);
		GD_wait_key_input();
	}


	//3、设置两个目标速度。正反向。
	if (1) {
		sm.new_frequency = -110;
		for (int i = 0; i < motor_len; i++) {

			if (i == motor_len / 4) {
				sm.new_frequency = 120;
			}
			if (i == (motor_len / 4) * 2) {
				sm.new_frequency = 0;
			}
			if (i == (motor_len / 4) * 3) {
				sm.new_frequency = -105;
			}

			y[i] = 0;
			while (1)
			{
				y[i]++;
				bool io_pulse = sm.io_pulse;
				steper_motor_clock(&sm);
				if (sm.io_enable == false)
				{
					break;
				}
				if (sm.io_pulse != io_pulse && sm.io_pulse) {
					break;
				}
			}
			//z[i] = sm.io_enable;
			z[i] = sm.io_dir;
			printfs(" \r\n  x = %9f z = %9f y = %9f , current step = %d  ", x[i], z[i], y[i], sm.current_step);

		}
		// 绘图显示。
		TwoFunctionPicture_draw_func(&tp,
			x, y, motor_len,
			x, z, motor_len,
			//	NULL, NULL, 0,
			1, 0, 0, NULL, NULL);
		printfs(" \r\n current step = %d current speed = %d ", sm.current_step, sm.current_frequency);
		GD_wait_key_input();
	}


	//4、先设置两个目标速度，最后回退到原点。
	if (1) {

		//	steper_motor_init(&sm);
		sm.new_frequency = 120;
		for (int i = 0; i < motor_len; i++) {

			if (i == motor_len / 5) {
				sm.new_frequency = -110;
			}
			if (i == (motor_len / 5) * 2) {
				sm.new_frequency = 0;
				//	sm.new_step = 0;
			}
			if (i == (motor_len / 5) * 3) {
				// sm.new_frequency = 0;
				sm.new_step = 0;
			}

			y[i] = 0;
			while (1)
			{
				y[i]++;
				bool io_pulse = sm.io_pulse;
				steper_motor_clock(&sm);
				if (sm.io_enable == false)
				{
					break;
				}
				if (sm.io_pulse != io_pulse && sm.io_pulse) {
					break;
				}
			}
			//z[i] = sm.io_enable;
			z[i] = sm.io_dir;
		}
		// 绘图显示。
		TwoFunctionPicture_draw_func(&tp,
			x, y, motor_len,
			x, z, motor_len,
			//	NULL, NULL, 0,
			1, 0, 0, NULL, NULL);
		printfs(" \r\n current step = %d current speed = %d ", sm.current_step, sm.current_frequency);
		GD_wait_key_input();
	}





}












